#include "../../Helix.h"
#include "../../HObject.h"
#include "../../Log.h"
#include "../../Math/HelixMath.h"
#include <ode/ode.h>
#include "Physics.h"
#include "ODEPhysics.h"
#include "PhysObjRepresentation.h"
#include "PhysObject.h"
#include "ODEPhysObject.h"

namespace helix
{

	ODEPhysics::ODEPhysics(Vector minWorldExtents, Vector maxWorldExtents) : Physics(minWorldExtents,maxWorldExtents)
	{
		dInitODE();
		world = dWorldCreate();
		space = dSweepAndPruneSpaceCreate(0,dSAP_AXES_XYZ);
		contactgroup = dJointGroupCreate(0);

		dWorldSetCFM (world, 1e-5);
		dWorldSetERP (world, 0.8);
		dWorldSetQuickStepNumIterations (world,20);

		dWorldSetGravity(world,0.0,-9.8,0.0);
	}
	
	ODEPhysics::~ODEPhysics()
	{
		dJointGroupDestroy (contactgroup);
		dSpaceDestroy (space);
		dWorldDestroy (world);
		dCloseODE();
	}

	PhysObject *ODEPhysics::createPhysObject(PhysRepresentation *rep,bool isStatic)
	{
		PhysObject *physObject = new ODEPhysObject(this,rep,isStatic);
		return physObject;
	}

	void ODEPhysics::deletePhysObject(PhysObject *physObj)
	{
		if(physObj != NULL)
			delete physObj;
	}

	void ODEPhysics::update(double dTime)
	{
		// check for collisions, create contact joints
		dSpaceCollide (space, this, &ODEPhysics::CollisionCallback);

		// step the world
		dWorldQuickStep (world, dTime);

		// remove all contact joints
		dJointGroupEmpty (contactgroup);

	}

	//The implementation of this callback is from the ODE wiki
	void ODEPhysics::CollisionCallback(void *pData, dGeomID o1, dGeomID o2)
	{

		if (dGeomIsSpace (o1) || dGeomIsSpace (o2)) {
			// colliding a space with something
			dSpaceCollide2 (o1,o2,pData,&ODEPhysics::CollisionCallback);
			// collide all geoms internal to the space(s)
			if (dGeomIsSpace (o1)) dSpaceCollide ((dSpaceID)o1,pData,&ODEPhysics::CollisionCallback);
			if (dGeomIsSpace (o2)) dSpaceCollide ((dSpaceID)o2,pData,&ODEPhysics::CollisionCallback);
		}
		else
			{

			// ignore collisions between bodies that are connected by the same joint
			dBodyID body1 = NULL, body2 = NULL;

			ODEPhysics *physics = (ODEPhysics*)pData;

			if (o1) body1 = dGeomGetBody (o1);
			if (o2) body2 = dGeomGetBody (o2);

			if (body1 && body2 && dAreConnected (body1, body2))
				return;

			
			dContact contact[MAX_CONTACTS];

			int iCount = dCollide (o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof (dContact));

			if (iCount)
			{
				int iMax = min (iCount, MAX_CONTACTS);
				for (int i = 0; i < iMax; i++)
				{
					contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1;
					contact[i].surface.mu = (dReal) 20.0;
					contact[i].surface.slip1 = 0.0;
					contact[i].surface.slip2 = 0.0;
					contact[i].surface.soft_erp = 0.8;
					contact[i].surface.soft_cfm = 0.01;

					dJointID contactJoint = dJointCreateContact (physics->world, 
																 physics->contactgroup, 
																 &contact[i]);

					dJointAttach (contactJoint, body1, body2);
				}
			}
		}

	}

}